/*
    This source file is part of GeNa project.
    Copyright © GeNa project 2008
    For the latest info, see http://code.google.com/p/genaproject/ or  http://lumpyproject.forums-rpg.com

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    Foobar is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Foobar.  If not, see <http://www.gnu.org/licenses/>. */

#ifndef MATRIX_H_INCLUDED
#define MATRIX_H_INCLUDED

#include <boost/operators.hpp>
#include <boost/serialization/array.hpp>
#include <boost/serialization/nvp.hpp>
using namespace boost;

#include "multiindicearray.h"

namespace math
{

template<class T> class Vector3;
template<class T> class Vector4;
template<class T> class Quaternion;

template<class T> class Matrix3 : equality_comparable< Matrix3<T>,
                            addable< Matrix3<T>,
                            subtractable< Matrix3<T>,
                            multipliable< Matrix3<T>,
                            multipliable< Matrix3<T>, T,
                            dividable< Matrix3<T>, T
                            > > > > > >
{
public:
    MultiIndiceArray<MultiIndiceArray<T,3>,3> m;
    T l[9];
    struct
    {
        T m11;
        T m12;
        T m13;

        T m21;
        T m22;
        T m23;

        T m31;
        T m32;
        T m33;
    };

    static const Matrix3 Identity;
    static const Matrix3 Null;

    Matrix3();

    Matrix3( T a11, T a12, T a13,
             T a21, T a22, T a23,
             T a31, T a32, T a33);

    template<class U> Matrix3( const Matrix3 &a);

    Matrix3( const Matrix3 &a) : l(&m11), m(&m11)
    {
        for( int i=0; i<9; i++)
            l[i] = a.l[i];
    }

    Matrix3 & operator=( const Matrix3 &a);

    //Operator
    bool operator==( const Matrix3 &a) const; //You should not use this test with floating point component !!!

    Matrix3 & operator+=(const Matrix3 &a);

    Matrix3 & operator-=(const Matrix3 &a);

    Matrix3 & operator*=(const Matrix3 &a);

    Matrix3 & leftProduct( const Matrix3 &a);

    Matrix3 & operator*=(T k);

    Matrix3 & operator/=(T k);

    template<class U> Vector3<U> operator*(const Vector3<U> &v) const;

    MultiIndiceArray<T,3> operator[](int n)
    {
        return m[n];
    }

    T det();

    Matrix3 inverse();

    Matrix3 transpose();

    template<class Archive> void serialize(Archive & ar, const unsigned int version);
};


template<class T> class Matrix4 : equality_comparable< Matrix4<T>,
                            addable< Matrix4<T>,
                            subtractable< Matrix4<T>,
                            multipliable< Matrix4<T>,
                            multipliable< Matrix4<T>, T,
                            dividable< Matrix4<T>, T
                            > > > > > >
{
public:
    struct
    {
        T m11;
        T m12;
        T m13;
        T m14;

        T m21;
        T m22;
        T m23;
        T m24;

        T m31;
        T m32;
        T m33;
        T m34;

        T m41;
        T m42;
        T m43;
        T m44;
    };
    T* l;
    MultiIndiceArray<MultiIndiceArray<T,4>,4> m;

    static const Matrix4 Identity;
    static const Matrix4 Null;

    Matrix4() : l(&m11), m(&m11)
    {
    }

    Matrix4( T a11, T a12, T a13, T a14,
             T a21, T a22, T a23, T a24,
             T a31, T a32, T a33, T a34,
             T a41, T a42, T a43, T a44);

    template<class U> Matrix4( const Matrix4<U> &a);

    Matrix4( const Matrix4 &a) : l(&m11), m(&m11)
    {
        for( int i=0; i<16; i++)
            l[i] = a.l[i];
    }

    Matrix4 & operator=( const Matrix4 &a);

    //Operator
    bool operator==( const Matrix4 &a) const; //You should not use this test with floating point component !!!

    Matrix4 & operator+=(const Matrix4 &a);

    Matrix4 & operator-=(const Matrix4 &a);

    Matrix4 & operator*=(const Matrix4 &a);

    Matrix4 & leftProduct( const Matrix4 &a);

    Matrix4 & operator*=(T k);

    Matrix4 & operator/=(T k);

    MultiIndiceArray<T,4> operator[](int n)
    {
        return m[n];
    }

    template<class U> Vector4<U> operator*(const Vector4<U> &v) const;

    template<class U> Vector3<U> transformPoint(const Vector3<U> &v) const;

    template<class U> Vector3<U> transformVector(const Vector3<U> &v) const;
    T det();

    Matrix4 inverse();

    Matrix4 transpose();

    Quaternion<T> toQuaternion();

    template<class Archive> void serialize(Archive & ar, const unsigned int version);
};

} //namespace math
#include "math/vector.h"
#include "math/quaternion.h"
namespace math
{

template<class T> Matrix3<T>::Matrix3() : l(&m11), m(&m11)
{
}

template<class T> Matrix3<T>::Matrix3(  T a11, T a12, T a13,
                                        T a21, T a22, T a23,
                                        T a31, T a32, T a33) : l(&m11), m(&m11)
{
    m11 = a11;
    m12 = a12;
    m13 = a13;

    m21 = a21;
    m22 = a22;
    m23 = a23;

    m31 = a31;
    m32 = a32;
    m33 = a33;
}


template<class T> template<class U> Matrix3<T>::Matrix3( const Matrix3 &a) : l(&m11), m(&m11)
{
    for( int i=0; i<9; i++)
        l[i] = a.l[i];
}

template<class T> Matrix3<T> & Matrix3<T>::operator=( const Matrix3 &a)
{
    for( int i=0; i<9; i++)
        l[i] = a.l[i];
    return *this;
}

//Operator
template<class T> bool Matrix3<T>::operator==( const Matrix3 &a) const //You should not use this test with floating point component !!!
{
    bool r;
    for( int i=0; i<9; i++)
        r = (r && (l[i] == a.l[i]));
    return r;
}

template<class T> Matrix3<T> & Matrix3<T>::operator+=(const Matrix3 &a)
{
    for( int i=0; i<9; i++)
        l[i] += a.l[i];
    return *this;
}

template<class T> Matrix3<T> & Matrix3<T>::operator-=(const Matrix3 &a)
{
    for( int i=0; i<9; i++)
        l[i] -= a.l[i];
    return *this;
}

template<class T> Matrix3<T> & Matrix3<T>::operator*=(const Matrix3 &a)
{
    Matrix3 a1 = const_cast<Matrix3&>(a);
    for( int i=0; i<3; i++)
    {
        T mi1 = m[i][0], mi2 = m[i][1], mi3 = m[i][2];
        for(int j=0; j<3; j++)
            m[i][j] = mi1*a1.m[0][j] + mi2*a1.m[1][j] + mi3*a1.m[2][j];
    }
    return *this;
}

template<class T> Matrix3<T> & Matrix3<T>::leftProduct( const Matrix3 &a)
{
    Matrix3 a1 = const_cast<Matrix3&>(a);
    for( int j=0; j<3; j++)
    {
        T m1j = m[0][j], m2j = m[1][j], m3j = m[2][j];
        for(int i=0; i<3; i++)
            m[i][j] = a1.m[i][0]*m1j + a1.m[i][1]*m2j + a1.m[i][2]*m3j;
    }
    return *this;
}

template<class T> Matrix3<T> & Matrix3<T>::operator*=(T k)
{
    for( int i=0; i<9; i++)
        l[i] *= k;
    return *this;
}

template<class T> Matrix3<T> & Matrix3<T>::operator/=(T k)
{
    for( int i=0; i<9; i++)
        l[i] /= k;
    return *this;
}

template<class T> template<class U> Vector3<U> Matrix3<T>::operator*(const Vector3<U> &v) const
{
    return Vector3<T>(  m11*v.x + m12*v.y + m13*v.z,
                        m21*v.x + m22*v.y + m23*v.z,
                        m31*v.x + m32*v.y + m33*v.z);
}

template<class T> T Matrix3<T>::det()
{
    return m11*m22*m33-m11*m23*m32+m21*m32*m13-m21*m12*m33+m31*m12*m23-m31*m22*m13;
}

template<class T> Matrix3<T> Matrix3<T>::inverse()
{
    T t4 = m11*m22;
    T t6 = m11*m23;
    T t8 = m21*m32;
    T t10 = m21*m12;
    T t12 = m31*m12;
    T t14 = m31*m22;
    T t18 = t4*m33-t6*m32+t8*m13-t10*m33+t12*m23-t14*m13;

    if( t18 == 0) //Singular matrix
        return Matrix3();

    T t17 = 1/t18;

    Matrix3 r;
    r.m11 = (m22*m33-m23*m32)*t17;
    r.m12 = -(-m32*m13+m12*m33)*t17;
    r.m13 = (m12*m23-m22*m13)*t17;
    r.m21 = -(m21*m33-m31*m23)*t17;
    r.m22 = (m11*m33-m31*m13)*t17;
    r.m23 = -(t6-m21*m13)*t17;
    r.m31 = (t8-t14)*t17;
    r.m32 = -(m11*m32-t12)*t17;
    r.m33 = (t4-t10)*t17;
    return r;
}

template<class T> Matrix3<T> Matrix3<T>::transpose()
{
    Matrix3 r;
    for( int i=0; i < 3; i++)
        for( int j=0; j < 3; j++)
            r.m[i][j] = m[j][i];
    return r;
}

template<class T> template<class Archive> void Matrix3<T>::serialize(Archive & ar, const unsigned int version)
{
    ar & serialization::make_nvp("l",serialization::make_array(l,9));
}
template<class T> Matrix4<T>::Matrix4(   T a11, T a12, T a13, T a14,
                                         T a21, T a22, T a23, T a24,
                                         T a31, T a32, T a33, T a34,
                                         T a41, T a42, T a43, T a44) : l(&m11), m(&m11)
{
    m11 = a11;
    m12 = a12;
    m13 = a13;
    m14 = a14;

    m21 = a21;
    m22 = a22;
    m23 = a23;
    m24 = a24;

    m31 = a31;
    m32 = a32;
    m33 = a33;
    m34 = a34;

    m41 = a41;
    m42 = a42;
    m43 = a43;
    m44 = a44;
}

template<class T> template<class U> Matrix4<T>::Matrix4( const Matrix4<U> &a) : l(&m11), m(&m11)
{
    for( int i=0; i<16; i++)
        l[i] = a.l[i];
}

template<class T> Matrix4<T> & Matrix4<T>::operator=( const Matrix4 &a)
{
    for( int i=0; i<16; i++)
        l[i] = a.l[i];
    return *this;
}

//Operator
template<class T> bool Matrix4<T>::operator==( const Matrix4 &a) const //You should not use this test with floating point component !!!
{
    bool r;
    for( int i=0; i<16; i++)
        r = r && (l[i] == a.l[i]);
    return r;
}

template<class T> Matrix4<T> & Matrix4<T>::operator+=(const Matrix4 &a)
{
    for( int i=0; i<16; i++)
        l[i] += a.l[i];
    return *this;
}

template<class T> Matrix4<T> & Matrix4<T>::operator-=(const Matrix4 &a)
{
    for( int i=0; i<16; i++)
        l[i] -= a.l[i];
    return *this;
}

template<class T> Matrix4<T> & Matrix4<T>::operator*=(const Matrix4 &a)
{
    Matrix4 a1 = const_cast<Matrix4&>(a);
    for( int i=0; i<4; i++)
    {
        T mi1 = m[i][0], mi2 = m[i][1], mi3 = m[i][2], mi4 = m[i][3];
        for(int j=0; j<4; j++)
            m[i][j] = mi1*a1.m[0][j] + mi2*a1.m[1][j] + mi3*a1.m[2][j] + mi4*a1.m[3][j];
    }
    return *this;
}

template<class T> Matrix4<T> & Matrix4<T>::leftProduct( const Matrix4 &a)
{
    Matrix4 a1 = const_cast<Matrix4&>(a);
    for( int j=0; j<4; j++)
    {
        T m1j = m[0][j], m2j = m[1][j], m3j = m[2][j], m4j = m[3][j];
        for(int i=0; i<4; i++)
            m[i][j] = a1.m[i][0]*m1j + a1.m[i][1]*m2j + a1.m[i][2]*m3j + a1.m[i][3]*m4j;
    }
    return *this;
}

template<class T> Matrix4<T> & Matrix4<T>::operator*=(T k)
{
    for( int i=0; i<16; i++)
        l[i] *= k;
    return *this;
}

template<class T> Matrix4<T> & Matrix4<T>::operator/=(T k)
{
    for( int i=0; i<16; i++)
        l[i] /= k;
    return *this;
}

template<class T> template<class U> Vector4<U> Matrix4<T>::operator*(const Vector4<U> &v) const
{
    return Vector4<T>(  m11*v.x + m12*v.y + m13*v.z + m14*v.w,
                        m21*v.x + m22*v.y + m23*v.z + m24*v.w,
                        m31*v.x + m32*v.y + m33*v.z + m34*v.w,
                        m41*v.x + m42*v.y + m43*v.z + m44*v.w);
}

template<class T> template<class U> Vector3<U> Matrix4<T>::transformPoint(const Vector3<U> &v) const
{
    return Vector3<T>(  m11*v.x + m12*v.y + m13*v.z + m14,
                        m21*v.x + m22*v.y + m23*v.z + m24,
                        m31*v.x + m32*v.y + m33*v.z + m34);
}

template<class T> template<class U> Vector3<U> Matrix4<T>::transformVector(const Vector3<U> &v) const
{
    return Vector3<T>(  m11*v.x + m12*v.y + m13*v.z,
                        m21*v.x + m22*v.y + m23*v.z,
                        m31*v.x + m32*v.y + m33*v.z);
}

template<class T> T Matrix4<T>::det()
{
    T t1 = m11*m22;
    T t2 = m33*m44;
    T t4 = m34*m43;
    T t6 = m11*m32;
    T t7 = m43*m24;
    T t9 = m23*m44;
    T t11 = m11*m42;
    T t12 = m23*m34;
    T t14 = m33*m24;
    T t16 = m21*m12;
    T t19 = m21*m32;
    T t20 = m43*m14;
    T t22 = m13*m44;
    T t24 = m21*m42;
    T t25 = m13*m34;
    T t27 = m33*m14;
    T t29 = t1*t2-t1*t4+t6*t7-t6*t9+t11*t12-t11*t14-t16*t2+t16*t4-t19*t20+t19*t22-t24*t25+t24*t27;
    T t30 = m31*m12;
    T t33 = m31*m22;
    T t36 = m31*m42;
    T t37 = m13*m24;
    T t39 = m23*m14;
    T t41 = m41*m12;
    T t44 = m41*m22;
    T t47 = m41*m32;
    T t50 = t30*t9-t30*t7+t33*t20-t33*t22+t36*t37-t36*t39-t41*t12+t41*t14-t44*t27+t44*t25-t47*t37+t47*t39;
    return t29+t50;
}

template<class T> Matrix4<T> Matrix4<T>::inverse()
{
    T t1 = m22*m33;
    T t3 = m34*m22;
    T t5 = m43*m32;
    T t7 = m23*m32;
    T t9 = m42*m23;
    T t11 = m42*m33;
    T t14 = m11*m22;
    T t15 = m33*m44;
    T t17 = m34*m43;
    T t19 = m11*m32;
    T t20 = m43*m24;
    T t22 = m23*m44;
    T t24 = m11*m42;
    T t25 = m23*m34;
    T t27 = m33*m24;
    T t29 = m21*m12;
    T t32 = m21*m32;
    T t33 = m43*m14;
    T t35 = m13*m44;
    T t37 = m21*m42;
    T t38 = m13*m34;
    T t40 = m33*m14;
    T t42 = t14*t15-t14*t17+t19*t20-t19*t22+t24*t25-t24*t27-t29*t15+t29*t17-t32*t33+t32*t35-t37*t38+t37*t40;
    T t43 = m31*m12;
    T t46 = m31*m22;
    T t49 = m42*m31;
    T t50 = m13*m24;
    T t52 = m23*m14;
    T t54 = m41*m12;
    T t57 = m41*m22;
    T t60 = m41*m32;
    T t63 = t43*t22-t43*t20+t46*t33-t46*t35+t49*t50-t49*t52-t54*t25+t54*t27-t57*t40+t57*t38-t60*t50+t60*t52;

    if( (t42 + t63) == 0) //Singular matrix
        return Matrix4();

    T t65 = 1/(t42+t63);
    T t67 = m12*m33;
    T t72 = m13*m32;
    T t74 = m42*m13;
    T t79 = m12*m23;
    T t85 = m22*m13;
    T t99 = m21*m33;
    T t101 = m21*m34;
    T t104 = m23*m31;
    T t108 = m24*m41;
    T t112 = m11*m33;
    T t114 = m11*m34;
    T t116 = m43*m31;
    T t118 = m31*m13;
    T t122 = m41*m13;
    T t126 = m23*m11;
    T t128 = m11*m43;
    T t133 = m21*m13;
    T t158 = m31*m14;

    Matrix4 r;
    r.m11 = (t1*m44-t3*m43+t5*m24-t7*m44+t9*m34-t11*m24)*t65;
    r.m12 = -(t67*m44-m12*m34*m43+t5*m14-t72*m44+t74*m34-t11*m14)*t65;
    r.m13 = (t79*m44-m12*m43*m24+m22*m43*m14-t85*m44+t74*m24-t9*m14)*t65;
    r.m14 = -(t79*m34-t67*m24+t1*m14-t85*m34+t72*m24-t7*m14)*t65;
    r.m21 = -(t99*m44-t101*m43+t25*m41-t104*m44+m24*m31*m43-t108*m33)*t65;
    r.m22 = (t112*m44-t114*m43+t116*m14-t118*m44-m41*m33*m14+t122*m34)*t65;
    r.m23 = -(t126*m44-t128*m24+t108*m13-m41*m23*m14-t133*m44+m21*m14*m43)*t65;
    r.m24 = (t126*m34-t112*m24+t118*m24-t104*m14-t133*m34+t99*m14)*t65;
    r.m31 = (t3*m41-t46*m44+t32*m44+t49*m24-t101*m42-m32*m24*m41)*t65;
    r.m32 = -(t19*m44-t114*m42+t158*m42+m34*m41*m12-t43*m44-t60*m14)*t65;
    r.m33 = (m11*m44*m22-t24*m24-m41*m14*m22-t29*m44+t54*m24+t37*m14)*t65;
    r.m34 = -(t114*m22-t101*m12-t158*m22-t19*m24+t32*m14+t43*m24)*t65;
    r.m41 = -(-t46*m43+t57*m33+t32*m43+t49*m23-t60*m23-t37*m33)*t65;
    r.m42 = (-t24*m33+t128*m32+t49*m13-t122*m32+t54*m33-t116*m12)*t65;
    r.m43 = -(t128*m22-m43*m21*m12-t122*m22-t24*m23+t37*m13+t54*m23)*t65;
    r.m44 = t65*(t14*m33-t126*m32+t32*m13-t29*m33+t43*m23-t46*m13);
    return r;
}

template<class T> Matrix4<T> Matrix4<T>::transpose()
{
    Matrix4 r;
    for(int i=0; i < 4; i++)
        for( int j=0; j < 4; j++)
            r.m[i][j] = m[j][i];
    return r;
}

template<class T> Quaternion<T> Matrix4<T>::toQuaternion()
{
    Quaternion<T> q;
    T tr = m11 + m22 + m33;

    if( tr < 0)
    {
        int i=0, j=1, k=2;
        if( m22 > m11)
        {
            i = 1; j = 2; k = 0;
        }
        else if( m33 > m[i][i])
        {
            i = 2; j = 0; k = 1;
        }

        T r = sqrt( m[i][i] - m[j][j] - m[k][k] + 1.0);
        T s = 0.5/r;

        q.l[i] = 0.5*r;
        q.l[j] = (m[i][j] + m[j][i])*s;
        q.l[k] = (m[k][i] + m[i][k])*s;
        q.w    = (m[k][j] - m[j][k])*s;
    }
    else
    {
        T r = sqrt(tr + 1.0);
        T s = 0.5/r;

        q.x = (m32 - m23)*s;
        q.y = (m13 - m31)*s;
        q.z = (m21 - m12)*s;
        q.w = 0.5*r;
    }
    return q;
}

template<class T> template<class Archive> void Matrix4<T>::serialize(Archive & ar, const unsigned int version)
{
    ar & serialization::make_nvp("l",serialization::make_array(l,16));
}

template< class T> const Matrix4<T> Matrix4<T>::Identity( 1,0,0,0,
                                                 0,1,0,0,
                                                 0,0,1,0,
                                                 0,0,0,1);

template< class T> const Matrix4<T> Matrix4<T>::Null( 0,0,0,0,
                                             0,0,0,0,
                                             0,0,0,0,
                                             0,0,0,0);

template< class T> const Matrix3<T> Matrix3<T>::Identity( 1,0,0,
                                                 0,1,0,
                                                 0,0,1);

template< class T> const Matrix3<T> Matrix3<T>::Null( 0,0,0,
                                             0,0,0,
                                             0,0,0);


} //namespace math
#include "math/fixed.h"

namespace math
{
typedef Matrix3<fixed> matrix3q;
typedef Matrix4<fixed> matrix4q;
typedef Matrix3<float> matrix3f;
typedef Matrix4<float> matrix4f;
} //namespace math


#endif // MATRIX_H_INCLUDED
